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ABSTRACT 


There  are  two  goals  for  autonomous  vehicle  navigation  planning:  shortest 
path  and  sate  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearance  between  the  vehicle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

We  use  the  derivative  of  curvature  k  of  the  vehicle  motion  (dK/ds)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  planning  of  the  autonontous  mobile  robot  Yamabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method.  Before  the 
mission  began  the  vehicle'was  given  a  track  to  follow;  motion  planning  consisted 
of  calculating  the  point  on  the  track  closest  to  the  vehicle  and  calculating  dK/ds 
then  steering  the  vehicle  to  get  onto  the  track. 

We  propose  a  method  of  planning  safe  motions  of  the  vehicle  to  calculate 
optimal  dK/ds  at  each  point  directly  from  the  information  of  the  world  without 
calculating  the  track  to  follow.  This  safe  navigation  algorithm  is  fundamentally 
different  from  path  tracking  using  a  path  specification.  Additionally,  motion 
planning  is  simpler  and  faster  than  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11  at  the  Naval  Postgraduate  School. 
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THESIS  DISCLAIMER 


The  reader  is  cautioned  th  ‘  .omputer  programs  developed  in  this  research 
may  not  have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been 
made,  within  the  time  available,  to  ensure  that  the  programs  are  free  of 
computational  and  logic  errors,  they  cannot  be  considered  validated.  Any 
application  of  these  programs  without  additional  verification  is  at  the  risk  of  the 
user. 
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EXECUTIVE  SUMMARY 

There  are  two  goals  for  autonomous  vehicle  navigation  planning:  shortest 
path  and  safe  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearances  between  the  vehicle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

We  use  the  derivative  of  curvature  k  of  the  vehicle  motion  {dKfds)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  planning  of  the  autonomous  mobile  robot  Yamabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method.  Before  the 
mission  began  the  vehicle  was  given  a  track  to  follow;  motion  planning  consisted 
of  calculating  the  point  on  the  track  closest  to  the  vehicle  and  calculating  dKjds 
then  steering  the  vehicle  to  get  onto  the  track.  The  safest  path  through  the  world 
navigated  by  the  vehicle  is  the  set  of  points  locally  maximizing  the  clearance  jGrom 
obstacles.  This  path  is  represented  by  the  Voronoi  diagram.  To  achieve  the  path 
tracking  m>^thod  it  is  necessary  to  calculate  the  Voronoi  boundary  which  consists 
of  line  segments  and  parabolic  arcs.  If  the  world  is  large,  it  is  complicated  and 
inefficient  to  calculate  every  Voronoi  boundary  of  this  world.  It  is  better  to 
calculate  optimal  dKjds  at  each  point  directly  from  the  information  of  the  world 
without  calculating  the  track  to  follow. 

When  the  objects  are  two  points,  two  lines,  or  one  point  and  one  line,  we  can 
safely  navigate  the  vehicle  to  achieve  equal  clearances  from  these  objects.  The 
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motion  of  the  vehicle  is  optimized  at  each  point  directly  from  the  information  of 
the  obstacles  near  the  vehicle.  After  calculating  dtc/ds,  the  vehicle  follows  the 
Voronoi  boundaries  defined  by  the  environment. 

Unlike  the  path  tracking  method,  this  method  can  be  applied  to  avoid  moving 
objects  since  we  calculate  the  optimal  motion  of  the  vehicle  at  each  point  directly 
from  the  information  of  the  world.  Additionally,  motion  control  is  simpler  and 
faster  than  in  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11  at  the  Naval  Postgraduate  School.  It  has  precise  knowledge  of 
its  location  in  a  given  environment  using  its  sonar  system.  The  robot  is 
programmed  by  the  high  level  mobile  robot  language  called  MML  (Model-based 
Mobile  robot  Language)  written  in  the  C  language. 
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I,  INTRODUCTION 


A.  BACKGROUND 

There  are  two  goals  for  planning  autonomous  vehicle  navigation  planning: 
shortest  path  and  safe  path.  These  goals  are  often  in  conflict;  path  safety  is  more 
important.  Safety  of  autonomous  vehicle  navigation  is  determined  by  the 
clearance  between  the  vehicle  and  obstacles.  Because  a  Voronoi  boundary  is  the 
set  of  points  locally  maximizing  the  clearance  from  obstacles,  safety  is  maximized 
on  it.  Therefore,  Voronoi  Diagrams  are  suitable  for  motion  planning  of 
autonomous  vehicles. 

B.  OVERVIEW 

We  use  the  derivative  of  curvature  *•  of  the  vehicle  motion  (dx/dr)  as  the 
only  control  variable  for  the  vehicle,  where  s  is  the  length  along  the  vehicle 
trajectory.  Previous  motion  platming  of  the  autonomous  mobile  robot  Yannabico- 
11  at  the  Naval  Postgraduate  School  used  a  path  tracking  method  [Ref.  2].  Before 
the  mission  began  the  vehicle  was  given  a  track  to  follow;  motion  planning 
consisted  of  calculating  the  point  on  the  track  closest  to  the  vehicle  and 
calculating  dKjds  then  steering  the  vehicle  to  get  onto  the  track.  The  safest  path 
through  the  world  navigated  by  the  vehicle  is  the  set  of  points  locally  maximizing 
the  clearance  from  obstacles.  This  path  is  represented  by  tlie  Voronoi  diagram.  To 
achieve  the  path  tracking  method  it  is  necessary  to  calculate  the  Voronoi 
boundary  which  consists  of  line  segments  and  parabolic  arcs.  If  the  world  is  large, 
it  is  complicated  and  ineffleient  to  calculate  every  Voronoi  boundary  of  this 
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world.  It  is  better  to  calculate  optimal  dKjds  at  each  point  directly  from  the 
information  cf  the  world  without  calculating  the  track  to  follow. 

This  safe  navigation  algorithm  is  fundamentally  different  from  path  tracking 
using  a  path  specification.  Additionally,  motion  planning  is  simpler  and  faster  than 
in  the  path  tracking  method. 

The  effectiveness  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  autonomous  mobile 
robot  Yamabico  11 4it  the  Naval  Postgraduate  School.  It  has  precise  knowledge  of 
its  location  in  a  given  environment  using  its  sonar  system.  The  robot  is 
programmed  by  the  high  level  mobile  robot  language  called  MML  (Model-based 
Mobile  robot  Language)  vmtten  in  the  C  language  [Ref.  3]. 
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II.  PROBLEM  STATEMENT 


The  problems  addressed  are  as  follows: 

1 .  How  to  navigate  the  robot  safely  to  achieve  the  same  clearance  from  two 
points. 

2.  How  to  navigate  the  robot  safely  to  achieve  the  same  clearance  from  two 
lines. 

3.  How  to  navigate  the  robot  safely  to  achieve  the  same  clearance  from  one 
point  and  one  line. 

4.  How  to  execute  the  safest  path  by  a  real  robot. 

Safety  is  one  of  the  important  attributes  for  autonomous  vehicle  navigation 
planning.  Safety  is  determined  by  the  clearance  between  the  vehicle  and  objects. 
Assume  the  vehicle  is  a  point  object  and  W{p)  is  the  clearance  of  the  point  p.  The 
clearance  of  a  path  represents  its  safety.  If  the  clearance  is  small,  the  path  is 
dangerous  because  it  is  close  to  the  object  and  if  it  is  larger,  the  path  is  safer.  The 

clearance  of  a  path  n  is  defined  as: 

W(XV)^MinW{,P)  (2.1) 

^«n 

Where  FI  is  the  path  of  the  vehicle  from  start  S  to  goal  G. 

We  want  to  find  the  path  rig  such  that  WCHg)  is  the  maximum  among  all 
possible  paths  (Figure  2.1).  To  maximize  the  clearance  1^(11),  we  will  take  the 
Voronoi  boundary  as  the  path  of  the  vehicle. 
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Figure  2.1 :  Safety  Path 


in.  VORONOI  DIAGRAM 


A.  DEFlNmONS 

Assume  that  there  is  an  object  o  in  a  plane.  It  might  be  a  point,  a  line,  a  line 
segment,  a  polygon,  or  other  closed  sets  of  points.  If  a  world  VJ  has  more  thaii  one 
object  a  Voronoi  region  of  an  object  o,  in  the  world  is  defuied  as 

V(o,)  =  l/’iy/Ii  y  ->  {distip,o, )  S  distip,Oj)]]]  (3.1) 

Where  p  is  a  point  which  is  not  inside  and  dist(p,o,  >  is  the  minimum 
distance  between  p  and  o,. 

The  set  of  all  the  Voronoi  regions  is  called  the  Voronoi  diagram  of  a  woild. 
Ihe  boundaries  of  Voronoi  regions  are  Voronoi  boundaries.  The  Voronoi 
diagram  of  a  geometric  world  typically  consists  of  lines,  rays,  line  segments,  and 
parabolic  arcs. 

B.  VORONC!  DUGRAM  OF  TTV'O  POINTS 

In  the  case  that  a  world  consists  of  two  distinct  points,  p^  and  pj*  Voronoi 
boundary  is  the  bisector  of  those  two  points  which  generate  two  Voronoi  regions 
Vipi)  and  ’/(pj)  (Figure  3.1). 

A=(-«i.y,) 

Voronoi  boundary  f 

I  ^  V(p,) 

_ i _ P _ 

V(p,) 

_ _ )>2=iX2,y2) _ 

Figure  3.1 :  Voronoi  Diagram  of  Two  Points 
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C  VORONOI  DIAGRAM  OF  TWO  LINES 


In  the  case  that  a  world  consist*:  of  two  lines  L,  and  which  are  not  parallel 
to  each  other,  their  Voronoi  boundaries  consist  of  the  bisectors  of  two  lines  which 
generate  eight  Voronoi  regions  V(L„).  V(L„),  V(LiJ,  ’/(Lj,),  V(/,j), 

V'(L2j)  and  V(L,J  (Figure  3.2). 


Figure  3.2  :  Voronoi  Diagram  of  Two  Lines  (Not  Parallel) 


D.  VORONOI  DUGRAM  OF  TWO  PARALLEL  LINES 

In  the  case  that  a  world  consists  of  two  parallel  lines  and  4,  their  Voronoi 
boundary  is  one  line  which  is  parallel  to  and  L,  which  generates  four  Voronoi 
regions  ^(1,,),  V(L,j),  V(Li,)  and  (Figure  3.3). 


^(A.) 

A - - 

Voronoi  boundary 


4 - - - 

_ 

Figure  33  :  Voronui  Diagram  of  Two  Parallel  Lines 


E.  VORONOI  DUGRAM  OF  A  LINE  SEGMENT 

In  the  case  that  a  world  consists  of  one  closed  line  segment  we  treat  it 
as  a  union  of  three  objects  :  two  end  points  p,,  pj  an  open  line  segment 
€  =  PjPj.  a  closed  line  segment  mcludes  both  endpoints,  but  an  open  line  segment 
does  not.  Therefore,  its  Voronoi  boundaries  consists  of  two  lines  which  generate 
four  Voronoi  regions  V(p,),  V(pj),  V(e.)  and  V(?2).  (Figure  3.4) 


Figure  3.4  Voronoi  Diagram  of  A  Line  Segment 
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F.  VORONOI  DIAGRAM  OF  A  POINT  AND  A  LINE 

In  the  case  that  a  world  consists  of  a  point  Pf  and  a  line  its  Voronoi 

boundary  is  a  parabola  which  generates  three  Voronoi  regions  V(p^),  Vie^),  and 
V(€i).  The  point  is  the/oc«j  and  the  line  is  the  directrix  of  the  parabola 

(Figure  3,5). 


Figure  3.5  :  Voronoi  Diagram  of  A  Point  and  A  Line 

G.  VORONOI  DUGRAM  OF  A  POINT  AND  TWO  LINES 

In  the  case  that  a  world  consists  of  a  point  p  that  is  between  two  parallel 
lines  I,  and  Z.,,  their  Voronoi  boundaries  arc  two  parabolas  (focus  p,  directrix  L, 
and  focus  p,  directrix  L^)  and  the  bisector  of  the  line  Z,  and  Z,  which  generate 
five  Voronoi  regions  V{p),  VCci),  VCCj),  V{e{)  and  ^(^4)  (Figures  3.6  and  3.7). 
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Fig  3.6  :  Voronoi  Diagram  of  A  Center  Point  and  Two  Lines 


Vie,) 

Vie,) 


parabola 


t  \  Vip) 

bisector  of 
Li  and  L2  ^ 

e,  parabola  Vie,) 


bisector  of 
Li  and  L2 


Figure  3.7  :  Voronoi  Diagram  of  A  Point  and  Two  Lines 

K.  VORONOI  DIAGRAM  OF  LINE  AND  A  RAY 

Assume  a  ray  is  a  kind  of  line  which  has  only  one  end  point  p .  In  the  case 
that  a  world  consists  of  a  line  I,  and  a  ray  which  is  orthogonal  to  the  line  L,, 
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their  Voronoi  boundaries  are  a  line  segment  and  two  bisec-tors  of  the  line  L, 
and  the  ray  L^,  and  a  parabola  (focus  p  and  directrix  I,),  which  generates  five 
Voronoi  regions  V(p),  VieJ,  Vie^),  Vie^)  and  V{e^)  (Figure  3.8). 


I  VORONOI  DIAGRAM  OF  TWO  LINES  AND  A  LINE  SEGMENT 

In  the  case  that  a  world  consists  of  two  parallel  lines  1^,1^  md  a  closed  line 
segment  pjPj,  which  is  parallel  to  lines  Z,  and  Lj,  their  Voronoi  boundaries  are 
bisectors  of  the  line  Z,  and  Z,,  the  bisector  of  the  line  Z,  and  the  line  segment 
P1P2,  the  bisector  of  the  line  Z,  and  the  line  segment  pjp,,  two  closed  line 
segment  and  pjPj  and  parabolas  (Figures  3.9  and  3.10). 
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Figure  3.10  :  Voroooi  Diagram  of  Two  Lines  and  A  Line  Segment 


11 


J.  VORONOI  DIAGRAM  OF  A  NORMAL  POLYGON 

In  the  case  that  a  world  has  only  one  normal  polygon,  we  treat  it  as  a  union 
of  vertices  (points)  p,  and  open  edges  e^.  There  are  Voronoi  boundaries  to  the 

polygon  shown  in  (Figure  3.11). 


V(A)  V{e,)  V(ft) 

Figure  3.11 :  Voronoi  Diagram  of  A  Normal  Polygon 


K.  VORONOI  DIAGRAM  OF  AN  INVERTED  POLYGON 

In  the  case  that  a  wt)rld  has  only  one  inverted  polygon,  we  also  treat  it  as  a 
union  of  vertices  (points)  p,  and  open  edges  e,  .  There  are  Voronoi  boundaries  in 

its  interior  (Figure  3.12). 


Parabola  1 


Figure  3.12  :  Voronoi  Diagram  of  An  Inverted  Polygon 
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IV.  CURVE  GENERATION 


A.  CONHGURATION 

To  navigate  a  rigid  body  robot  vehicle,  the  vehicle’s  state  can  be  described  by 
its  current  configuration, 

q  =  {p,9)  (4.1) 

where  p  is  the  vehicle's  current  coordinate  position  (x,y),  and  6  is  the  vehicle’s 
tangent  orientation  at  that  point 

Let  V  be  the  derivative  of  tangent  orientation  with  respect  to  the  length 
along  the  trajectory  s  which  is  called  cui/ature. 


)c(s)  = 


ddjs) 

ds 


(4.2) 


In  this  case,  k  is  not  included  in  the  configuration.  However,  k  can  be  included 
in  the  configuration  if  ic  is  required  for  the  calculation. 


B.  NEXT  FUNCTION 

In  order  to  compute  the  sequence  of  conriguradons,  it  is  suffice  to  compute 
the  next  configuration  at  each  step  Ay.  Given  Ay  and  estimate  the 

vehicle's  next  configuration  at  j+  Ay  as  follows. 

1.  Short  Circular  Segment 

The  short  path  segment  between  s  and  J+Ay  is  approximated  by  a  circular 
curve  segment  (Figure  4.1). 


I 

1  15 


/ 


Figure  4.1 :  Short  Circular  Segment 


Assume  configurations  of  both  endpoints  are  and  q^. 

<?o  =  (Uo»yo).®o)  =  ((0»0),0) 

Let  us  assume  Ad  9^  0.  The  radius  of  the  short  circular  segment  is 


Where 


As 

Ad 

Ad  =  di  -  do 


(4.3) 

(4.4) 

(4.5) 

(4.6) 
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The  configuration  of  the  end  point  is 


9i  = 


V  ; 


(f  rsin(A0) 

^Kl-cosCAC))^ 

Ae 


( (  (sin(A0)/A0)As 
J(1  -cos(A0))/A0)A5 


9i  = 


J 


/'/'qW 

.o> 

vO  , 


AO 


(if  Afl  =  0) 


(ifAe^tO)  (4.7) 


(4.8) 


Using  the  Taylor  expansion  forms  for  the  sin  and  cos  functions 


sin(Aa)  _  Ag-  (Ae)V3!+  (Aef/5\+  _  ^  jAPf  ^  (AO)* 
Ad  “  Ad  ~  3!  5! 

1  -  cos(Ad)  _  1  -  (1  -  (Ad) V2!+  (Ad)74!-  (Ad)V6!-  -) 
Ad  Ad 


(4.9) 


=r±-<^+(^_...V« 

1,2!  4!  6!  f 

Tlierefore,  the  configuration  of  the  point  is 


(f'r  V 

Y  (l-(Ad)V3!  +  (Ad)V5!  — OAs  V 

^1  = 

u> 

|,(l/2!  -  (Ad)V4!  +  (Ad)V6!  -  •  OAdAs  J 

1  Ad  J 

L.  general, 

Ad  «1 


Therefore  equation  (4.1 1)  can  be  approximated  as 


^1  = 


(l-(Ad)V3!)Ay 

(l/2!-(Ad)V4!)AdAs 

Ad 


y 


(4.10) 


(4.11) 


(4.12) 


(4.13) 
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2.  Global  Position  Calculation 

By  using  the  composition  function  o  [Ref.  1]  which  is  a  two  dimensional 
coordinate  transformation  from  the  local  coordinate  system  (Xpyj)  to  the  global 
coordinate  system  (a:o.}'o)«  can  calculate  the  global  position  of  the  vehicle  at 
s+Aj  as  follows. 


q{s  +  As)  =  qoO  q^  = 


^Xo  +  jc,  cos  ©0  -  >1  sin  $0 '' 
yo  +  xsinSo+yiCosOo 


I  + 

Where  q^  and  are  given  Equation  (4.3)  and  Equation  (4.13). 


(4.14) 
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V.  SAFE  NAVIGATION  USING  A  STEERING  FUNCTION 


We  use  t’.e  derivative  of  curvature  as  the  only  control  variable  for  the 
vehicle. 


(5.1) 

ds 

Therefore  the  vehicle's  motion  is  controlled  only  through  changing  its  curvature. 

Ar  =  (^]Ar  (5.2) 

We  propose  the  steering  function  in  the  following  form 

—  =  -(aAK+bAe  +  cM)  (5.3) 

ds 

or 

die 

— +aAir+M0+cAd  =  O  (5.4) 

ds 

where  a,b,c  are  positive  constants  and  Air,  A0,  Ad  are  variables.  Each 
evaluation  of  Air,  Ad,  Ad  differs  in  the  situation  of  the  obstacles  (in  the  case  that 
the  obstacles  are  one  point  and  one  line,  the  obstacles  are  two  lines,  and  the 
obstacles  are  two  points). 

A.  LINE  TRACKING 

Consider  a  special  case  of  the  line  tracking  (Figure  5.1).  Assume  we  want  to 
track  the  X-axis  of  the  global  coordinate  system,  then  we  can  define  three 
positive  constants  a,b,c . 
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Figure  5.1 :  Line  Tracking 


On  the  X-axis, 


=  ««.=0 


Therefore 

AK=K-Ki„  =  K 

A9=e-e„  =  e 

Ad  =  y 

Let  us  now  consider  a  short  path  segment  (Figure  5.2). 


Figure  5.2  :  Short  Path  Segment 


The  vehicle  s  tangent  orientation  6  is  specified  by 

tan0  =  — 

Ax 

Then 

6  =  tan"'  —  =  tan"'  y'  (as  Ax  -4  0) 
Ax 

(y)%(yy 

^35 

As  is  defined  as 

As=^j(Axf+iAy? 

Therefore 


(5.9) 


(5.10) 

(5.11) 


From  the  definition 

_d  . 

—(tan"' y )  = -^^  = —^^ 
From  Equation  (5.12)  and  Equation  (5.13) 


(5.12) 


(5.13) 


ds  d  ^(tan"'y) 

K  =  —  =  — (tan"'  y )  =  -^ 
ds  ds 


y 


_ ^  i+(y)"  ^  y 


Therefore 


(5.14) 


dK 


-j 

^ =  y ” (1 + (y  f  )"^  - 3y  (y •  )"(i + (y  f  (5.15) 

ds  EL  Vl  +  (y) 
dx 
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Assume 


.<  1 

(5.16) 

y(y)^<  y" 

(5.17^ 

Then  from  Equation  (5.10) 

A3  =  y' 

(5.18) 

From  Equation  (5.14) 

AK  =  y" 

(5.19) 

From  Equation  (5.15) 

dK  _  , 

ds  ^ 

(5.20) 

Equation  (5.4)  becomes  an  ordinary  differential  equation; 

y'"+ay''+by'+cy  =  0 

(5.21) 

(DUaD^  +  bD  +  c)y  =  0 

(5.22) 

Since  this  is  a  third  order  linear  homogeneous  ordinary  differential  equation  with 
constant  coefficients,  it  must  have  at  least  one  real  root.  If  it  has  a  non-negative 
root,  it  does  not  have  a  converging  solution.  If  it  has  a  complex  conjugate  root, 
the  solution  oscillates  even  if  it  decays.  Since  we  want  non-oscillatory  decaying 
solutions.  Equation  (S.22)  must  have  three  negative  roots  of  D.  Also  if  we  want  a 
critical  damping  solution  then  Equation  (5.22)  must  be  specified  to  have  a  triple 
root,  -k  (where  k  >  0). 

D^+aD^+bD  +  cs{D  +  kf  =D^  +  3kD^  +  3k}  D  +  k^  (5.23) 

Therefore  if  we  choose 

a  =  3k  (5.24) 

b  =  3k^  (5.25) 

c  =  k^  (5.26) 


Equation  (5.22)  becomes 
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{D-^k)^y-=0  (5.27) 

Thus,  under  this  condition,  there  is  only  one  degree  of  freedom  in  choosing  the 
parameter  k  instead  of  three  parameters  a,  b  and  c.  We  defme 


S' 


(5.28) 


Hiis  size  constant  So  controls  the  distance  for  which  the  vehicle  runs  before  it 


gets  on  track.  A  smaller  size  constant  makes  the  transition  distance  smaller.  Thus 
So  controls  the  sharpness  of  the  trajectory.  From  Equations  (5.3),  (5.24),  (5.25), 

(5.26)  and  (5.28),  the  revis'‘4  steering  function  becomes 


dK 

ds 


\Ak+3\ 


A0  +  3 


M 


(5.29) 


Air,  Ad,  6d  are  evaluated  depending  upon  the  environment.  Let  consider 
three  situations  for  vehicle  navigation:  the  objects  are  two  points,  the  objects  are 
two  lines,  the  objects  are  one  point  and  one  line. 


B.  TWO  POINTS 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  two 
points,  its  trajectory  becomes  a  line  which  is  a  bisector  of  the  two  points.  It  is  a 
Voronoi  boundary  of  the  two  points.  Let  consider  the  case  of  the  world  consists 
of  two  points  Pi  and  pj  (Figure  5.3), 
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Pl=(x^,y^) 

_ P2=U2.y2) _ 

Figure  5.3  :  Safe  Navigation  of  Two  Points 

L  Evaluation  of  AK 

When  the  vehicle's  configuration  is  ^  =  (p,0,  ir), 

Lk  =  k  (5.30) 

Since  final  value  of  k  on  the  Voronoi  boundary  is  zero. 

2.  Evaluation  of  A6 

Let  denote  the  orientation  from  p  to  p,  and  'PCp.Pj)  denote 

the  orientation  from  p  to  pj.  Let  a  be  the  difference  between  the  orientation 
y(p,Pi)  and  the  vehicle's  orientation  0;  p  is  the  difference  between  the  vehicle's 
orientation  0  and  'FCp.pj)  (Figure  5.4). 


a  =  'F(p.p,)-0 

/?  =  0-'P(p,Pj) 


(5.31) 

(5.32) 


When  vehicle  is  on  the  Voronoi  boundary, 

a  =  p  (5.33) 

Let  the  desired  orientation  be  6^.  Kl  the  start  point 

9,  -y(p,p,)W(p,p,)  (5.34) 

Where  normalizel  is  a  function  which  normalizes  its  argument  into  a  range  of 
[~T’t]  necessary  and  Po  is  the  middle  point  between  pj 

and  Pj.  At  the  other  point, 

9,  (5.35) 

Where  6^^  is  the  vehicle's  previous  desired  orientation  6^  at  the  point.  Then  the 
variable  A0  is  evaluated  as 

i^e=e-e^  (5.36) 

3,  Evaluation  of  Ad 

Let  d^  be  the  distance  between  p  tmd  p,,  and  dj  be  the  distance  from  p 

to  pj . 

di  =  4^x-Xyf-^{y-y^f  (5.37) 

dj  =-^/u-Xj)^  +  (y-yj)^  (5.38) 

The  signed  variable  Ad  is  evaluated  as  follows  (Figures  5.5  and  5.6).  Note  that 
Ad  can  be  signed  positive  or  negative,  or  equal  zero. 
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Pl=U^,y^) 


Figure  5.5  ;  Case  'J'(p,p,)-'I'(p,P2)>0 


P2=(^2.>’2) 


q  =  (p,d,K) 

_ Pi=Ui>yi) _ I 

Figure  5.6  :  Case  'P(p,p,)-'4'(p,P2)<0 

M  =  (if  4'(p,p,)-'P(P,P2)>0)  (5.39) 

=  (if¥(p,p,)-V(p,P2)<0)  (5.40) 

4.  Simulation  Results 

The  use  of  this  steering  function  for  vehicle  motion  control  is 
demonstrated  by  algorithmic  simulation  and  by  use  on  the  existing  robot 
Yamabico  11.  The  result  is  shown  in  Figures  5.7  and  5.8.  Figure  5.7  shows  the 
case  p,  =(0,100),  p^  =  (0,-100),  the  initial  configuration  of  the  vehicle  is 
^  =  ((-300, 50),  0,0),  where  there  are  eight  cases  of  the  initial  0:0  45,  90,  135, 
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180,  225,  270,  315  degrees.  Figure  5.8  shows  the  case  where  the  initial 
configuration  of  the  vehicle  is  q  =  ((-300, -50),  0,0). 
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Figure  5.8  ;  Simulation  Result  of  Two  Points,  q  =  ((-300, -50), 0,0) 


C.  TWO  LINES 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  two 
lines,  its  trajectory  becomes  a  line  which  is  a  bisector  of  two  directed  lines.  So  it  is 
a  Voronoi  boundary  of  two  lines.  Assume  a  world  consists  of  two  directed  lines 
and  gj,  there  are  two  cases:  they  are  parallel  or  not  parallel  (Figures  5.9  and  5.10). 
Interestingly,  calculations  for  Ajc,  A0  and  Ad  are  identical  in  each  case. 
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Figure  5.10  :  Not  Parallel  Directed  Lines 


1.  Evaluation  of  Ak 

When  the  vehicle's  configuration  is  ^  =  (p,  6,  k)  , 

Sk=k  (5.41) 

Since  final  value  of  jc  on  Voronoi  boundary  is  zero. 

2.  Evaluation  of  A6 

When  the  vehicle  is  on  the  Voronoi  boundary  its  orientation  is  the 
average  of  6^  and  Let  this  desired  orientation  be  8^, 
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=  normalizel^^^^  “  ^  (5  -42) 

Where  normalize  1  is  a  function  which  normalizes  its  argument  into  a  range  of 
by  addition  of  ±nn  if  necessary.  Then  the  variable  Ad  is  evaluated  as 

Ae  =  e-e^  (5.43) 

3.  Evaluation  of  Ad 

Let  d,  be  the  signed  distance  from  p  to  and  dj  signed 

distance  from  p  to  q^. 

di  =-(x-j:i)sindj  +  (y-y,)cos0i  (5.44) 

dj  =-(x-X2)sin02+(y-y2)cos02  (5.45) 

The  signed  variable  Ad  is  evaluated  as 

Ad=^i^  (5.46) 

The  signed  distance  from  p  to  is  the  distance  between  p  and  p 
is  on  the  left  of  q^,  then  d^  >0  and  if  p  is  on  the  right  of  q^,  then  dj  <0  (Figure 
5.1 1).  A  similar  argument  holds  for  . 


p 

k 

,>oT 

d.<oT 

9, 

T 

P 

Figure  5.11 :  Signed  Distance  from  p  to  q, 
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4.  Simulation  Result 

The  result  of  algorithmic  simulation  can  be  found  in  Figures  5.12,  5.13, 
5.14,  and  5. 15. 

Figures  5.12  and  5.13  show  the  case  where  the  lines  are  parallel.  Figure 
5.12  shows  the  case  =((0,100),0,0),  =((0»“100),0,0)  and  the  initial 

configuration  of  the  vehicle  is  ^  =  ((0,50),  ^,0),  where  there  are  eight  cases  of  the 
initial  0  ;  0,  45,  90,  135,  180,  225,  270,  315  degrees.  Figure  5.13  shows  the  case 
where  the  initial  configuration  of  the  vehicle  is  ^  =  ((0,-50),  0,0) . 

Figures  5.14  and  5.15  show  the  case  where  the  lines  are  not  parallel. 
Figure  5.14  shows  the  case  ^,  =  ((0,0),90,0),  =  ((0.0),0,0)  and  the  initial 

configuration  of  the  vehicle  is  q  =  ((50,150), 6,0).  Figure  5.15  shows  the  case 
where  the  initial  configuration  of  the  vehicle  is  q  =  ((150,50),  6,0). 


Figure  5.12  :  Simulation  Result  of  Parallel  lines,  q  =  ((0,50), 6,0) 
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Figure  5.15  ;  Simulation  Result  of  Not  Parallel  Lines,  q  =  ((150, 50), 6,0) 


D.  ONE  POINT  AND  ONE  LINE 

When  we  navigate  the  vehicle  safely  to  make  the  same  clearance  from  one 
point  and  one  line,  its  trajectory  becomes  a  parabola.  So  it  is  a  Voronoi  boundary 
of  a  point  and  a  line. 

1.  Definition  of  Parabola 

When  a  world  consists  of  a  point  p,  and  a  directed  line  its  Voronoi 
boundary  becomes  a  parabola.  A  parabola  is  defined  as  the  focus  Pj  and  the 
directrix  : 

Pf-i.Xj,yf)  (5.47) 

^o=(Wo.®o)  (5.48) 

The  directrix  has  a  direction,  and  hence,  parabola  has  a  direction.  Let 
1  be  the  signed  distance  from  to  q^ . 
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1  =  -U/  -jro)sii‘^o  +  (>’/  -3’o)cos0o  (5.49) 

The  signed  distance  from  to  is  the  distance  between  and  -  P/  is  on 
the  left  of  ^0 ,  then  1>  0  and  if  p^is  on  the  right  of  q^ ,  then  1<  0  (Figure  5.16). 


Pf 

A 

_ H _ 

^0^ 

KOI 

T 

Pf 

Figure  5.16  :  Signed  Distance  from  to  q. 


a.  Case  1>  0 

Let  0,  denote  the  orientation  of  the  normal  ray  from  p,  to  q^ .  We 
define  a  polar  coordinate  system  whose  pole  is  p^  and  whose  initial  ray  is  6^ 

(Figure  5.17). 


Figure  5.17  :  Parabola  (l  >  0) 
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d,=do-7tl2  (5.50) 

In  this  system,  p  is  represented  by  where  r  is  the  distance  between  and  p 
and  0  is  the  orientation  from  Pf  to  p  taken  from  the  initial  ray.  In  this  case,  we 
take  0(-;r<  0<  n)  counterclockwise  from  the  initial  ray.  The  coordinate  of  p  in 

the  global  Cartesian  system  is 

(x,y)  =  {Xf  +  rcosCe,  +  0),y/  +  rsin(fi,  +  0)) 

,  {.  +  tfl]  (5^1) 

^  1  +  COS0  ^  1  +  COS0 

Let  'V{p^,p)  denote  the  orientation  from  p^  to  p.  By  definition,  the  angle  a 
between  4'(p^,p)  and  the  orientation  of  the  tangent  at  p  is  defined  as 


2  2 

The  orientation  'F  of  the  tangent  at  p  in  the  polar  coordinate  system  is 


(5.52) 


=  =  =  (5.53) 

The  orientation  $  of  this  tangent  at  p  in  the  global  coordinate  system  is 

e=6,+H'=(e,-.|)+[|+|]=|+e.  (5.54) 

Let  s  denote  the  arc  length.  Then  the  curvature  k*  at  p  is 


I 
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b.  Case  l<0 

In  this  case  (Figure  5.18),  the  orientation  6^  of  the  initial  ray  in  the 
global  coordinate  system  is 


01  =eo  +  ;r/2 


(5.56) 


Figure  5.18  :  Parabola  ( l<  0) 


We  take  (ft  (- k  <  <p  <  k)  clockwise  from  the  initial  ray. 

(p  =  -(P’  (5.57) 

Then  the  coordinate  of  p  in  the  global  Cartesian  system  is  defined  by  Equation 
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(5.51),  the  orientation  6  of  the  tangent  at  p  in  the  global  coordinate  svstem  is 
defined  by  Equation  (5.55)  and  the  curvature  k  at  p  is  defined  by  Equation 
(5.56). 

2.  Evaluation  of  Ak 

When  the  vehicle's  configuration  is  q  =  {,p,6,K),  assume  the  intersection 
of  the  orientation  4'(p,p,)  and  the  parabola  is  ^  shown  in 

Figure  5.19.  The  variable  Ak-  is  evaluated  as 

Ak-k-k^  (5.58) 

Note  that  Ak  converges  to  zero  as  q  approaches  to 


T 

6i  =  0o-n/2 


Figure  5.19 :  Evaluation  of  Ak 
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3.  Evaluation  of  A6 

Let  a  be  the  difference  between  the  vehicle's  orientation  9  and  the 
orientation  of  the  initial  ray.  Also  let  p  be  the  difference  between  the 
orientation  }  and  the  vehicle's  orientation  6  (Figure  5.20). 


t  i 

0^  =9^- nil 


Figure  5.20  :  Evaluation  of  A6 


Then 


a  =  H>{p,p^)-9 

(5.59) 

p  =  9-e, 

(5.60) 

When  vehicle  is  on  the  parabola. 

a  =  p 

(5.61) 

Let  this  desired  orientation  be  9^, 
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9^  =  normalizel^ — j  ^  ^5  ^2) 

Where  normalizel  is  a  function  which  normalizes  its  argument  into  a  range  of 
j^-y,yj  by  addition  of  ±nn  if  necessary.  TTien  the  variable  A0  is  evaluated  as 

Ad  =  9-0,  (5.63) 

4.  Evaluation  of  Ad 

Let  d,  be  the  distance  between  p  and  p^,  and  dj  the  signed  distance 
from  p  to  Qf,  (Figure  5.21). 

di=^(x-Xyy  +  (y-y^f  (5.64) 

dj  =-(x-j:o)sindo  +  (y-yo)cosao  (5.65) 


9^=9o-^/2 


Figure  5.21 :  Evaluation  of  Ad 


(5.66) 

(5.67) 


The  signed  variable  Ad  is  evaluated  as 
Ad  =  d2-di  (if  l>0) 

Ad  =  d2+d,  (if  KO) 
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5.  Simulation  Result 

The  result  of  algorithmic  simulation  can  be  found  in  Figures  5.22,  5.23, 
5.24  and  5.25.  Figures  5.22  and  5.23  show  the  case  where  the  1  is  positive. 

Figures  5.24  and  5.25  show  the  case  where  the  1  is  negative. 

Figure  5.22  shows  the  case  of  Pf  =(0,200),  =((0,0), 0,0),  the  initial 

configuration  of  the  vehicle  is  ^  =  ((-300, 200),  0,0),  where  there  are  eight  cases  of 
the  initial  6  :  0,  45,  90,  135,  180,  225,  270,  315  degrees.  Figure  5.23  shows  the 
case  where  the  initial  configuration  of  the  vehicle  is  ^  =  ((-200, 300),  0,0) . 

Figure  5.24  shows  the  case  of  =(0,-200),  =  ((0,0),0,0),  the  initial 

configuration  of  the  vehicle  is  ^  =  ((-300, -200),  0,0),  where  there  are  eight  cases 
of  the  initial  0  :  0,  45,  90, 135, 180, 225,  270,  315  degrees.  Figure  5.25  shows  the 
case  where  the  initial  configuration  of  the  vehicle  is  ^  =  ((-200,-300),  0,0) . 
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Figure  5.25  :  Simulation  Result  of  1<0,  q  =  ((-200, -300), 0,0) 
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VI.  CONCLUSION 


A.  RESULTS 

Simulation  results  shows  that  when  the  objects  are  two  points,  two  lines,  or 
one  point  and  one  line,  we  can  safely  navigate  the  vehicle  to  achieve  equal 
clearances  from  these  objects.  The  motion  of  the  vehicle  is  optimized  at  each 
point  directly  from  the  information  of  the  obstacles  near  the  vehicle.  After 
calculating  the  steering  function  dKjds  which  is  the  derivative  of  the  curvature  of 
the  vehicle  motion,  the  vehicle  follows  the  Voronoi  boundaries  defined  by  the 
environment 

Previous  work  in  the  motion  control  of  the  autonomous  vehicle  Yamabico  11 
used  path  tracking  using  a  path  specification  for  lines,  circles  and  parabolas 
which  are  images  of  the  path.  Before  tlie  mission  began  the  vehicle  was  given  a 
track  to  follow;  motion  planning  consisted  of  calculating  the  point  on  the  track 
closest  to  the  vehicle  and  then  steering  the  vehicle  to  get  onto  the  track. 

If  the  world  navigated  by  the  robot  is  large,  it  is  complicated  and  inefficient  to 
calculate  every  Voronoi  boundary  of  this  world.  It  is  better  to  calculate  the 
optimal  motion  of  the  vehicle  at  each  point  arectly  from  the  information  of  the 
world  by  computing  the  steering  function  dxjds  without  calculating  the  track  to 
follow. 

Unlike  path  tracking  method,  this  method  can  be  applied  to  avoid  moving 
objects  since  we  calculate  the  optimal  motion  of  the  vehicle  at  each  point  directly 
from  the  information  of  the  world.  Additionally,  motion  control  is  simpler  and 
faster  than  in  the  path  tracking  method. 
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B.  RECOMMENDATIONS 

Based  on  the  results  of  this  thesis,  recommended  follow  on  work  includes 
leaving  point  calculation. 

There  are  two  goals  for  planning  autonomous  vehicle  navigation  planning; 
shortest  path  and  safe  path.  This  safe  navigation  method  is  for  only  safe  path 
planning.  The  short  path  planning  is  represented  by  path  tracking  using  a  path 
specification  for  lines,  circles  and  parabolas  which  are  images  of  the  path.  When 
we  will  combine  this  safe  navigation  method  with  short  path  planning,  it  will  be 
necessary  to  calculate  the  leaving  point  from  one  path  to  another.  Afterwards,  the 
vehicle  will  continue  on  its  way  smoothly. 
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APPENDIX 


This  appendix  contains  the  C  code  for  safe  navigation  which  generated  the 
results  found  in  this  thesis. 


A.  POINTPATH.C 

Author:  Masahide  Shirasaka 
Project:  Yamabico  Robot  Control  System 
Date:  June  25  1994 

Revised:  July  12  1994 

File  Name;  pointpath.C 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 

Description:  This  Program  contains  functions  for  safe  navigation 

when  the  obstacles  are  two  points. 

4c4c4c4c4e4c4c4i4i4c4c4i4t4c4<#4<###################4<#####################^ 

#include  <stdio.h> 

#include  <math.h> 

#defme  DR  (PI/180.0) 

#define  PI  3.14159265358979323846 
//  =PI 

#defme  DPI  6.28318530717958647692 
//  =2.0*PI 

#defme  HPI  1.57079632679489661923 
//  =  PI/2.0 

#define  RAD  57.29577951308232087684 
//  =  180.0/PI 

FILE  ^fpO,  *11)1; 

y4c####4c#4t##4<4c4<4<#  ################################  ########### 

Struct:  POINT 

4c4c4c4c4c4c#4c#4c  ##############  4>########4c4c4e#4c4c#4>#4c4c####4c4t4c4cv4c4c4c4<4cy 

typedef  struct  { 
double  x; 
double  y; 

} 

POINT; 

y#4c4c4(4c4c4<4<4c4>4c4c4'4'*l<  ######################  4c4c4c4c4c4‘##4c4<4c#4c4c4c4c4c4c#4c4c 

Struct:  CONFIGURATION 
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typedef  struct  { 

POINT  point; 
double  theta; 
double  kappa; 

} 

CONHGURATION; 

Ht^c^it:ttiteii^i^t:tt^eJt‘#*^t***********’t‘**:¥*!^**i‘****)ic***!t‘************’it** 

Function;  normalize() 

Purpose:  This  proc^ure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  •  PI  values. 

double  nonnali2e(double  angle) 

{ 

angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  - 1.0); 
retum(angle); 

} 


lilc:t‘*itt^t*ilt***Hi^c*^c*******Hi*ihitt*itt***ttt:ttHi:^)tt^c^cHc)tc*^i*m*************** 

Function:  noimalizelO 

Purpose:  a  ..is  procedure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  -  PI/2.0  values. 

ic4c:tc:t‘*’¥************^*********^*******’i‘*‘*’i‘********  **********  I 

double  normalizel  (double  angle) 

{ 

whi)e(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <=  -PI/2.0) 

{ 

angle  -  angle  +  PI; 

} 

retum(angle); 

} 

lilf^tiltHc^*^i*ittii-.iliilfi)cHiitt^titiJlt/:Mf*************************************** 

FUNCTION;  InputPointsO 

Purpose:  This  procedure  Inputs  the  configurations  of  two  points. 

%  4c  %  %  41 4<  %  4:  >ti  <|I  4>  4i  *  4i  >)>  4>  4>  4.  )t<  4<  4<  4>  %  4<  a|>  %  *  %  %  Itt  41 %  4>  %  <!<  )|t  ■<>  III  %  Ik  4c  %  4i  %  41  %  41 4<  4<  / 

void  InputPoints(POINT  &pl4*OINT  &p2) 

{ 

/*  Point  obstacle  pi  */ 

printfC'Input  Coordinates  of  the  pi.  Nn"); 

printf("X=V."); 

scanf(”%lf',&pl.x); 
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printf("Y=\n"); 

scanf("%r,&pl.y): 

/*  Point  obstacle  p2  */ 

printf(”Input  Coordinates  of  the  p2.Nn"); 

piintf("X=Nn"); 

scanf("%lf’,&p2.x); 

printf("y=Nn"); 

scanf("%lf',&p2.y); 

} 

FUNCTION;  InputlnitConfigO 

Purpose:  This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 

size  constant  and  step  size. 

:ti  :(i  %  jft  %  4t  )|i  %  %  4t  4c  4e  %  4<  4i  4i  1)1 4i  ]tc  4c  ]tc  4i  4c  %  4i  ii<  4<  4<  :<<  )|c  *  %  >tt  *  >|i  ^  lit  lit  i|t  i|<  I’:  ^  4i  *  *  4c  ik  >it  4c  ^ 

void  InputInitConfig(CONFIGURATION  &q, double  &30,double  &DS) 

{ 

/*  Config  of  q  */ 

printfC'Input  initial  Configuration  of  the  vehicle  q.  Nn”); 

printf('’X=Vn"); 

scanf("%lf',&q,pointx); 

ptintf("Y=\n"); 

scanf("%lf',&q.pointy); 

printf("theta= 

scanf("%ir,&q.theta); 

q.theta=noiTnalize(q.theta/RAD); 

printf("kappa=  Nn"); 

scanf("%lf",&q.kappa); 

/♦  Size  constant  */ 

printf("Input  the  size  constant  sCNi"); 
piintfC'sO  =Nn"); 
scanf("%lf’.&sO); 

/♦  DS  */ 

printf("lnput  the  step  size  DS.Nn"); 
printfC'DS  =Nn"); 
scanf("%r,&DS); 

} 


y  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  ^c  4>  4c  4> «  4e  4' 4c  4c  4<  4c  4c  4c  41 4c  4c  4c  4>  4c  4c  4c  4c  4c  41  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetInitThetaDesire() 

Purpose:  This  pi "  redure  is  for  a  fr  :*tion  which  compute  the  value  of 

desires  :dal  theta. 

4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c »  -  <  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c »  «  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 

double  GetInitThetaDesire(CONFIGURA’nON  qJPOINT  pi  J*OINT  p2) 
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{ 

POINT  pO; 

pO.x  =  (pi  .X  +  p2.x)/2.0: 
pO.y  =  (pl.y  +  p2.y)/2.0: 

retum(normalizel((atan2(pl  .y-q.point.y,pl  .x-q.point.x) 

+  atan2(p2.y-q.point.y,p2.x-q.point.x))/2.0 
-  atan2(p0.y-q.point.y,p0.x-q.point.x) ) 

+  atan2(p0.y-q.point.y,p0.x-q.point.x)); 

} 

FUNCTION:  GetConstants() 

Purpose:  This  procedure  is  for  a  function  which  compute  the  value  of 

constants  k,  a,  b,  and  c. 

*  4c  *  4c  *  *  ak  *  4c  ^  4< «  *  Ik  4<  4<  4<  %  ♦  4> «  itt  *  *  *  )*>  tit  41 «  «  Xt  If  %  *  *  *  He  * 

void  GetConstants(double  S0,double  &a, double  &b,double  &c) 

{ 

double  ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3  .O^ConstK^ConstK; 

c=ConstK*ConstK*ConstK; 


y  4<  4c «  4c  4c  4c  4c  4c  4c  4c  4c  4c  4e  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  He  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4e  4c  4c  4c  4c  4c  *  4c  He  *  4c  4c  4c  4c  4c  4c  4c  4c  4i  4c  He  4c  4c  * 

FUNCTION:  GetSteeringFunc() 

Purpose:  This  procedure  is  for  a  function  which  compute  the  value  of 

steering  function  dk/ds. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4i  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4i  4c  4c  4c  4c  4c  4c »  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 

void  GetSteering(double  a,double  b,double  c.CONFIGURATION  q, 

POINT  pl4*OINT  p2,double  &thetaDesire,double  &u) 

{ 

double  deltaKappa,deltaTheta,deltaDist,dl  ,d2; 

/*  Calculate  deltaKappa  */ 
deltaKappa  =  q.kappa; 

/*  Calculate  deltaTheta  */ 

thetaDesire  =  normalizel((atan2(pl.y-q.point.y,pl.x-q.point.x) 

+  atan2(p2.y-q.poini.y,p2.x-q.point.x))/2.0 
-  thetaDesire)  +  thetaDesire; 
deltaTheta  =  normalize(q.theta  -  thetaDesire); 

/*  Calculate  deltaDist  */ 

dl  =  sqrt((pl.xq.point.x)*(pl.x-q.pomt.x) 
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+  (pl.y-q.point.y)'''(pl.y-q.pointy)); 
d2  =  sqrt((p2.x-q.poLnt.x)*(p2.x-q.point.x) 

+  (p2.y-q.point.y)*(p2.y-q.point.y)); 
if  (atan2(pl  .y-q.point.y,pl.x-q.point.x) 

-  atan2(p2.y-q.point.y,p2.x-q.pointx)>0) 
deltaDist  =  d2-dl; 
else 

deltaDist  =  dl  -  d2; 


I*  Calculate  Steering  fucnction  =  u  */ 
u  =  -(a*deltaKappa  +  b*deltaTheta  +  c^deltaDist); 

} 

FUNCTION:  GetDkappa() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dKappa. 

*  *  4c  4t  *  4c  *  *  4c »  *  *  *  4<  4c  *  4c  4c  *  *  *  *  *  *  *  4c  *  )|t  4c  4c  4c :|c  *  ])i  4c  :tc  4c  9|c  4c  It  4c « !|c  y 

double  GetDk;appa(double  u, double  ds) 

( 

retum(u*ds); 

} 


y  4c  4c  4c  4c  4c  41 4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 


FUNCTION:  GetKappaO 

Purpose:  This  procedure  is  for  a  hinction  which  computes  the  value  of 

Kappa. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c «  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 


void  GetK2q)pa(double  dkjq)pa,C0NFIGURAT10N  &q) 
{ 

q.kappa=q.k^a  +  dkappa; 

} 


y  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4i  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetDthetaQ 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dtheta. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c «  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  >!<  4c  4c  4c  4c  4c  4c  y 

double  GetDtheta(CONFIGURATION  ql, double  ds) 

{ 

retum(ql  .kappa  *  ds); 

} 


y  4c  4c  4c  4c  4c  4c  4>  4c  4c  4t  4c  4c  4c  4<  *  4c  4>  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c 

FUNCTION:  next() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  next 

configration  of  the  vehicle. 
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void  next(double  ds, double  dtheta,doubIe  &s, CONFIGURATION  &q) 

CONHGURATIONql; 

/*  CONHGURATION  of  ql  */ 
ql.pointx  =  (1.0  -  dtheta*dtheta/6.0)*ds; 
ql.point.y  =  (0.5  -  dtheta*dtheta/24.0)*dtheta*ds; 
ql. theta  =  dtheta; 

s  =  s  +  ds' 

/*  CONFIGURATION  of  q  */ 

q.pointx  =  q.pointx  +  ql.point.x*cos(q.theta)  -  ql.point.y *sin(q.theta); 
q.pointy  =  q.point.y  +  ql.point.x*sin(q.  theta)  +  ql.point.y*cos(q.  theta); 
q.theta  =  q.theta  +  ql. theta; 

} 

^4(4c4c*4t*4i****4i*4i4t«*4i*)tt**4c4>»*4c**:ii«***4c*4c**  ******** 

FUNCTION:  Openfile() 

Purpose:  This  procedure  opens  the  output  file. 

******************************************************** **^ 
void  Openfile(CONFIGURATION  q,double  s) 

{ 

fpO  =  fopen(''path.dat","w"); 
f^)!  -  fopen("path","w"); 

fprintf(ff)0,"  s  X  y  theta[deg]  kappa  "); 

fprintf(^,"  u  deltaK2q)pa  deltaTheta  deltabis^n"); 

printf("  s  X  y  Aeta[deg)  kappaNn"); 

fpiintf^(fp0,"%4.4f  ^.4f  %4.4f  %4.4f  %4.4Ni",s,  q.point.x,  q.pointy, 

q.theta*RAD,q.kappa); 

printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f^",  s,  q.pointx,  q.pointy, 

q.theta’'‘RAD,q.kappa); 
fprintf(^l,"%f  %f'n",  q.pointx,  q.pointy); 

} 

^*******************7^************************************** 

FUNCTION:  PrintfileQ 

Purpose:  This  procedure  prints  the  result  to  the  file. 

****************************************************** ****y 

void  Print!ile(CONFIGURATION  q,double  s) 

fprintf(^,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f  ", 

s,  q.pointx,  q.pointy,  q.theta*RA.D,q.kappa); 
printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4fVn", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
^rintf(fpl,"%f  %fVn",  q.point.x,  q.pointy);  /*  for  gnuplot  */ 
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P^UNCnON:  mainQ 

#  %  4i  #  *  *  1)1  4e  >ti  :<<  4<  *  4<  *  itt  *  *  4c  41  *  it«  4e  *  *  4i  *  *  *  *  4c  4i  >|i  )ti  *  4<  itc  *  >^  #  >^  *  # ’)' *  *  * it<  it<  *  ^  4<  I 

void  inain(void) 

{ 

CONHCURATIONq; 

POINT  pi  ,p2; 

double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c,thetaDesire; 
double  dkappa.dtheta; 

InputPoints(p  1  ,p2); 
lnputInitCoiifig(q,sO,DS); 

GetConstants(sO,a,b,c); 

thetaDesire=GetInitThetaDesire(q,pl  ,p2); 

s  =  0.0; 

Openfile(q,s); 

do 

{ 

GetSteering(a,b,c,q,pl  ,p2,thetaDesire,u); 
dkappa  -  GetDkappa(uJDS); 

GetKappa(dkappa,q); 

dthcta=GetDtheta(q,DS); 

next(DS,dtheta,s,q); 

Prin^e(q^); 

} 

while(s<=800,0); 

fclose(fpO); 

fclosc(fpl); 

} 


B.  LINEPATH.C 

^  4c  4c  4c  4c  4<  4c  4c  4c  4>  4c  4c  4c  4c  4c  4- 4c  4c  4>  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4i  4c  4c  4c  4c  4c  4: 41 4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4: 4c 

Author:  Masahide  Shirasaka 

Project:  Yamabico  Robot  Control  System 

Date:  June  26  1594 

Revised:  July  12  1994 

File  Name:  linepath.C 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 
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Description:  This  Program  contains  functions  for  safe  navigation 

when  the  obstacles  are  two  directed  lines. 

iti  ********  ****************:tc************  *************  / 


#include  <stdio.h> 

#include  <math.h> 

#derme  DR  (PI/1 80.0) 

#define  PI  3.14159265358979323846 
//  =PI 

#derme  DPI  6.28318530717958647692 
//  =2.0*PI 

#defmeHPI  1.57079632679489661923 
//  =  PI/2.0 

#define  RAD  57.29577951308232087684 
//  =  180.0/PI 

FILE  *fip0,  *fpl; 

Struct:  POINT 

********************************************************** I 

typedef  struct  { 
double  x; 
double  y; 

} 

POINT; 

Struct:  CONFIGURATION 

typedef  struct  { 

POINT  point; 
doable  theta; 
double  kappa; 

} 

CONHGURATION; 

y :tc 4c :tc 4citc :ic :tc 4c % 4i 4c 9|! 4c  4c 4c :(( 4: i|c 4c 4: 4c 4c 4c g|i j(c 4c 9t< 4< ^ il< iti sic !<< 4c !|t 4-. * ifc 4:  4c 4c )tc 

Function:  nomnalize(angle) 

Purpose:  This  procedure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  -  PI  values. 

4'4c4'4'4'4c*4'4c4c4'4'4c4c4c4c4'4c4'4'4'*4'4c4'4c4c4t4t4c4t4t4c4c4c4c4c4t4c4t4;4c4c4t4t4c4c4c4'4c4t4'4c4t4t4c4'4>y 

double  normalize(double  angle) 

{ 

angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  - 1.0); 
retum(angle); 

} 
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Function:  normalize  1  (angle) 

Purpose:  This  procedure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  -  PI/2.0  values. 

4c  4e  *  *  *  *  ♦  *  *  *  itc  *  *  *  4c  *  iti  *  *  4^  %  *  3|<  41 4c  *  He  itc  3|i  4t  4c  4c  %  41  :|C  4<  *  4c  *  *  *  *  *  *  sic  ifc  ]|C  4c Itc  4c  >l<  4c  *  :(c  :tc  y 

double  normalize  1  (double  angle) 

{ 

while(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <-  -PI/2.0) 

{ 

angle  =  angle  +  PI; 

} 

retum(angle); 

} 

^  4c  4c  4c  4c  4‘ 4c  4c  4c  41 4c  4c  4c  4c  4c  41 4c  4c  4>  4‘ 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  *  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  InputLinesO 

Purpose:  This  procedure  Inputs  the  configurations  of  two  Lines. 

4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4<  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4: 4c  4c  4c  4c  ale  y 

void  InputLines(CONHGURATION  &ql.CONnGURATION  &q2) 

{ 

/*  Line  obstacle  ql  */ 

printf("lnput  initial  Configuration  of  ql.  Nn"); 

printf(''X=Nn"); 

scanf("%lf ',&ql  .pointx); 

printf("Y=Nn"); 

scanf("%lf '  ,&qLpoint.y); 

orintf("theta='ii"); 

scanf("%r,&ql  .theta); 

ql  .theta=normalize(ql  .tlieta/RAD); 

ql  .kappa=0.0; 

/*  Line  obstacle  q2  */ 

printf("Input  initial  Configuration  of  q2.  '^"); 

printf("X=Nn"); 

scanf("%lf',&q2.point.x); 

printf(”Y=Nn"); 

scanf("%lf',&q2,point.y); 

printf("theta=  '^' ); 

scanf("%lf',&q2.theta); 

q2.theta=nonnalize(q2.theta/RAD); 

q2.kappa=0.0; 

} 
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FUNCTION:  InputlnitConfigO 

Purpose:  This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 

size  constant  and  step  size. 

void  InputInitConfig(CONFIGURATION  &q, double  &s0, double  &DS) 

{ 

/*  Config  of  q  */ 

printfC'Input  initial  Configuration  of  the  vehicle  q.  \n"); 

printf("X=V); 

scanf("%lf',&q.poinLx); 

printf("Y=V’); 

scanf(''%lf',&q.poinLy); 

printf("theta=  Nn"); 

scanf("%lf',&q.theta); 

q.theta=normalize(q.theta/RAD); 

printf("kappa=  Nn"); 

scanf("%lf',&q.kappa); 

/*  Size  constant  */ 

printfC'Input  the  size  constant  sChn"); 
piintfC’sO  =Nn"); 
scanf("%lf',&sO); 

/♦  DS  */ 

printfC'Input  the  step  size  DS.Nn"); 
printffDS  =Nn"); 
scanf("%r,&DS); 


y  in  4i  >•(  **«  Id’ll  *****%  4c  >|i  iK  4c  %  4c  Id  >•<  4^  X<  %  4c  Xi  ^  )|c  *  <1 *  4c  « 9(e  4<:(>  **  i|c  ** 

FUNCllON:  GetConstants() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

constants  k,  a,  b,  and  c. 

iK  4c  Id  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  Id  Id  4  4c  4c  4c  Id  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4cy 

void  GetConstants(doiible  S0,double  &a,double  &b, double  &c) 

{ 

double  ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3.0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 

y  4c  41 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4|  Id  4c  4c  4c  4c  4c  41 4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  Id  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  Id 

FUNCTION:  GetSteeringFuncO 
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Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

steering  function  dk/ds. 

double  GetSteering(double  a,double  b.double  c,CONFIGURATION  q, 
CONHGURAllON  ql,CONHGURATION  q2) 

{ 

double  deltaKappa,deltaTheta,deltaDisuthetaDesire,dl  ,d2; 

/*  Calculate  DeltaKappa  */ 
deltaKappa  =  q.kappa; 

/*  Calculate  DeltaTheta  */ 

thetaDesire  =  normalizel((ql.theta+q2.theta)/2.0  -  ql. theta)  +  ql .theta; 
deltaTheta  =  normalize(q.theta  -  thet^esire); 

/*  Calculate  DeltaDist  */ 

dl  =  -(q.pointx  -  ql.point.x)*sin(ql. theta) 

+  (q.point.y  -  ql  .iwint.y)*cos(ql  .theta); 
d2  =  -(q.pointx  -  q2.pointx)*sin(q2.theta) 

+  (q.point.y  -  q2.pointy)*cos(q2.theta); 
deltaDist  =  (dl-Hd2)/2.0; 

/♦  Calculate  Steering  fucnction  =  u  */ 
retum(-(a*deltaKappa  +  b*deltaTheta  +  c^deltaDist)); 

} 

FUNCTION:  GetDkappa() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dKappa. 

4c  *  4c  *  4t  *  4c  *  *  4c  4<  4c  4i  *  *  *  *  4c  *  *  %  >t<  >11  ^  *  He  4c *  Xc  Id  *  *  *  *  *  y 

double  GetDkappa(double  u,double  ds) 

{ 

retum(u*ds); 

} 

y  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  41 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetKappa() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

Kappa. 

4c  4c  %  4c  V  4c  4c  4c  4c  4<  4c  Id  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4‘ 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  41 4c  y 

void  GetKappa(double  dk;appa,C0NFIGURAT10N  &q) 
q.kappa=q.kappa  +  dkappa; 

} 

y  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 
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FUNCTION:  GetDtheta() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dtheta. 

***)tt*i:*******:tc*tc^t******^t*^et)t*****nc**  ***********************  I 

double  GetDtheta(CONFIGURATION  ql, double  ds) 

{ 

retum(ql. kappa  *  ds); 

} 

y  :(c  ]4e  4c  4:  *  *  *  >4:  *  *  If  iK  *  4: <<  4: 4: 4c  4:  *  4c  i|t  4c  *  *  4^  *  *  ik  iti  ifc  3)c  :(c  ifc  :tc  :(i  i(c  :tc  i|c  I):  !(c  %  :4c  ifi  % 

FUNCTION:  next() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  next 

configration  of  the  vehicle. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  41 4c  4c  4c  4<  4c  4c  4t  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  41 4c  4c  4‘ 4c  4c  4c  *  4c  4c  4>  4c  4c  4c  4c  4c  4c  I 

void  next(double  ds, double  dtheta,dotibIe  &s,C0^4FIGURATI0N  &q) 

{ 

CONHGURATIONql: 

/*  CONHGURATION  of  ql  */ 
ql.point.x  =  (1.0  -  dtheta*dtheta/6.0)*ds; 
ql.point.y  =  (0.5  -  dtheta *dtheta/24.0)*dtheta*ds: 
ql.  theta  =  dtheta; 


s  =  s  +  ds; 


I*  CONHGURATION  of  q  */ 

q.pomtx  =  q.pointx  +  ql  .point.x*cos(q.theta)  -  ql.point.y*sin(q.theta); 
q.pointy  =  q.pointy  +  ql.pointx*sm(q.theta)  +  ql.point.y*cos(q.theta); 
q.theta  =  q.theta  +  ql. theta; 

1 

y  4c  4c  4c  4c  4c  4c  4c  4c  4e  4c  4c  4c  4c  4c -#  4c  4c  4c  4c  4c  4c  4c  4c  4i  4c  4i  4c  4i  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4*  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  Jpenfile() 

Purpose:  This  procedure  opens  the  ouqput  file. 

4c  4i  4c  4c  4:  If  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  cf  4>  4<  4<  4<  4c  4>  4i  4c  4c  4c  4c  4<  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4i  4c  4c  4i  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c^ 

void  Openfile(CONHGURATION  q.double  s) 

{ 

fpO  =  fopenf"path.dat","w"); 
fj)!  =  fopen("path","w"); 

fprintf(f^,''  s  X  y  theta[deg]  kappa  "); 
fprintf(f^,"  u  deltaKt^pa  deltaTheta  deltaDisfsn"); 
printf("  s  X  y  ^eta[deg]  kappaNn"); 
fprintf(f^,"%4.4f  ^.4f  %4.4f  %4.4f  %4.4fNn",s,  q.point.x,  q.point.y, 
q.theta*RAD,q.kappa); 

printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4fm",  s,  q.point.x,  q.pointy, 

q  .theta*R  AD,q.kappa); 
fprintf(fpl,"%f  %f\n",  q.pointx,  q.pointy); 
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} 

******♦♦♦♦****♦*♦*♦**♦♦*★♦*  *♦*♦♦♦*♦♦** 

FUNCTION;  Printfile(,) 

Purpose:  This  procedure  prints  the  result  to  the  file. 

4e  4<  ift  #  4t  <<  *  %  *  4c  %  4  aft  :tc  >t>  3t<  1)1  *  *  *  lit  Ik  *  %  %  i|c  >•>  iK  %  4=  %  >*1  *  *  il:  itc  %  *  li' *  *  y 

void  Printfile(CONFIGURATION  q,double  s) 

{ 

fprintf(lp0.''%4.4f  %4.4f  %4.4f  %4.4f  %4.4f  ", 

s,  q.pointx,  q.point.y,  q.theta*RAD,q.kappa); 
prmtf("%4.4f  %4.4f  %4.4f%4.4f  %4.4fSn", 

s,  q.poinUx,  q.point.y,  q.theta*RAD,q.kappa); 
fpiintf(fpl,"%f  %fVn'',  q.point.x,  q.point.y);  /*  for  gnuplot  */ 

} 

^4e  4c  it  4e  4c  4t  4c  4c  lit  etc  4c  41 4<  4c  4c  4c  *  ^c  4c  *  4c  4c  4c  3|e  4c  4c  4>  4c  4t  4c  4c  4c  4c  *  4c  4t  4i  4c  4c  itc  4c  4c  4c  4c  4c  4c  *  4c  4c :)( 4c  4e  ^c  4c  *  4c  4c  4c 

FUNCTION:  tnainQ 

4c  4c  4c  4c  4c  4c  4(  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4e  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 

void  main(void) 

{ 

CONHGURATION  q,ql,q2; 
double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c; 
double  dkappa,dtheta; 

lnputLines(ql  ,q2); 

lnputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

s  =  0.0; 

Openfile(q,s); 

do 

{ 

u  =  GetSteering(a,b,c,q,ql,q2); 
dkappa  =  GetDkappaCuJDS); 

GetKappa(dkappa,q); 

dtheta=GetDtheia(q,DS); 

next(DS,dtheta,s,q); 

Prin^e(q,s); 

} 

while(s<=800.0); 

fclose(fpO); 

fclose(fpl); 

} 
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C  PARAPATH.C 


4c  itc  4c  4c  4c  %  :tc  ift  #  :)i  4e  %  i|c  ift  itc  *  #  )|c  4c  i|c  :tt  4c  :|E  i|c  ifc  %  Dtcitc  %  %  i|c  ik  *  *  %  itcft  itc*  :)c  :tc  *  4c  :)( 

Author:  Masahide  Shirasaka 

Project:  Yamabico  Robot  Control  System 

Date:  May  15  1994 

Revised:  June  17  1994 

File  Name:  parapath.C 

Environment:  GCC  ANSI  C  compiler  for  the  motorola  68020  processor 

Description:  This  Program  contains  functions  for  safe  navigation 

when  the  obstacles  are  one  point  and  one  directed  line. 

4c4c4c4c4c*4c4c4c4c4c4c4c4c4c4>4c4>4'4‘4!4c4i4c4c4<4c4c4c4c4c4>ctc4c4c4c4c4c4c4c4c4c4‘%4<4c4c4c4c4c4c4c4c4c4c4c4c4c^ 

#include  <stdio.h> 

#include  <math.h> 

#defme  DR  (PI/180.0) 

#define  PI  3.14159265358979323846 
//  =PI 

#defme  DPI  6.28318530717958647692 
//  =2.0*PI 

#defme  HPI  1.57079632679489661923 
//  =  PI/2.0 

#define  RAD  57.29577951308232087684 
//  =  180.0/PI 

FILE  ^fpO,  ♦fpl,  *fp2,  *fp3; 


^4c  4c  4c «  *  *  *  «  4c  4c 4!  4c  *  4c «  4c  4c  4c  4e  4c  4c  4<  4c  4c  4c  4c  4c4c4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  tc  4c  4c  4c  4c  4>  etc  *  4c  4c  4c  4c  4c  *  4c 

struct:  POINT 

4c  4c  4c  4c  4c  4c  4c  4c  4!  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4(  4c  4c  4>  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  ^ 

typedef  struct  { 
double  x; 
double  y; 

} 

POINT; 

^4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4‘ 4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

Struct:  CONFIGURATION 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4<  4c  *  4>  4>  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  yT 

typedef  struct  { 

POINT  point; 
double  theta; 
double  kappa; 

) 

CXDNHGURATION; 
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Function:  normalize() 

Purpose:  This  proc^ure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  -  PI  values. 

***********^:Dt*:tt*:tt*1li1ltm*****-********************************l 

double  normalize(double  angle) 

{ 

angle  =  angle  -  DPI*(ceil((angle  +  PI)/DPI)  -  1.0); 
retum(angle); 

} 

Function:  normalizel() 

Purpose:  This  procedure  is  for  a  function  which  normalizes  an  angle 

to  within  +  or  -  PI/2.0  values. 

double  normalizel  (double  angle) 

{ 

while(angle  >  PI/2.0) 

{ 

angle  =  angle  -  PI; 

} 

while(angle  <=  -PI/2.0) 

{ 

angle  =  angle  +  PI; 

} 

retum(angle); 

} 

y  %  4<  4i  %  )|>  4<  %  4<  4>  %  4>  4>  %  4i  4<  4>  4<  >l>  41 4<  4<  4i  ^  4>  *  4c  4<  4>  4>  4>  *  4' %  4<  4i  #  4i  *  *  Ik  *  4c  :•>  4c  #  :|t  ^ 

FUNCTION:  InputParabolaO 

Purpose:  This  procedure  Inputs  the  Configrations  of  one  point  and 

one  directed  line. 

4c  4i  4c  *  4c  4c  4i  4c  4<  4c  4(  4c  4t  4i  4t  4i  4c  4i  4<  4c  4c  4c  4i  4i  4c  4<  4c  4c  4c  4c  4i  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4t  4  4c  4c  4c  4c  4c  4c  y 

void  InputParabola(  CONFIGURATION  &q04*OINT  &p) 

{ 

/*  Config  of  qO  */ 

printfC'Input  initial  Configuration  of  the  qO  (directrix).^”); 

printf("X=Vi"); 

scanf("%lf',^q0.poiRt.x); 

piintf("Y=V'); 

scanf("%lf',5^0.point.y); 

printf("theta=  Nn"); 

scanf("%lf',&q0.theta); 

q0.theta=normalize(q0.theta/RAD); 

q0.kappa=0.0; 
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/*  Point  obstacle  */ 

printf("Input  Coordinates  of  the  pf  (focus).  Nn"); 

printf("X='n’'); 

scanf("%lf,&p.x); 

printf("Y=Nn”); 

scanfC’%lf',&p.y): 

} 

FUNCTION;  InputlnitConfigO 

Purpose:  This  procedure  Inputs  the  initial  Configration  of  the  vehicle, 

size  constant  and  step  size. 

4c  *  )k  i|c  It:  :1c  *  4<:|c  4c  4<  4c  Ik  4c  4c  4: 4i  ^  4e  :|t  4c  4t  :|e  4c  4i  *  4c  itc  *  *  !k  *  *  *  4t  *  :•(  4c  4c  4cite  4i  *  ^ 

void  InputInitConfig(CONFIGURATION  &q, double  &s0, double  &DS) 

{ 

/*  Config  of  q  */ 

printfC'Input  initial  Configuration  of  the  vehicle  q. 

printf("X=Nn"): 

scanf("%lf',&q.poinLx); 

printf("Y=V); 

scanf("%lf',&q.pointy): 

printf("theta=  Nn"): 

scanf("%lf',&q.thcta); 

q.theta=nonnaJize(q.theta/RAD); 

printf("kappa=  Nn"); 

scanf("%lf',&q.kappa); 

/*  Size  constant*/ 

printfC'Input  the  size  constant  sCNn"); 
printfC’sO  =Nn"); 
scanf("%lf',&sO); 

/*  DS  */ 

printf("Input  the  step  size  DS.Nn"); 
printfC'DS  =Nn"); 
scanf("%lf',&DS); 

} 

^  4c  4c  4c  4c  4: 4c  4c  4: 4c  4c  4c  4c  4c  4c  4c  4: 4c  4c  4i  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetSize() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

size  of  the  parabola. 

4c  4c  4c  4c  *  4c  4c  4c  4c  4c  4c  4: 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  j 

double  GetSize(CONFIGURATION  qO  J*OINT  p) 

retum(-(p.x-q0.point.x)*sin(q0.theta)  +  (p.y-q0.point.y)*cos(q0.theta)); 
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FUNCTION:  GetConstants() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

constants  k,  a,  b,  and  c. 

*  *  *  *  *  4c  *  4c  *  %  4c  4c  *  4>  *  lit  %  itc  i|c  iti  :te  4c  >i<  4c  4c  1(1 4c  4t  *  * 4c  *  *  III  3k  *  9)1 4t  4>  %  i(<  *  *  *  *  %  ^ 

void  GetConstants(doubIe  SO, double  &a,double  &b, double  &c) 

{ 

double  ConstK; 

ConstK=1.0/S0; 

a=3.0*ConstK; 

b=3 .0*ConstK*ConstK; 

c=ConstK*ConstK*ConstK; 

} 

^4c  4c  4c  4c  4t  4c  4c  4>  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4t  4c  4' 4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4e  4c 

FUNCTION:  GetSteeringFunc() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

steering  function  dk/ds. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4t  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4>  4c  %  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 

double  GetSteering(double  a,double  b,double  c.CONFIGURATTON  q, 
CONFIGURATION  qO^^OINT  p, double  size) 

{ 

double  kappaPara,phi,deltaKappa,thetaN,thetaDesire, 
deltaTheta,deltk)ist,dl  ,d2; 

/*  Calculate  DeltaKappa  ♦/ 
if  ( size  >=  0.0 ) 
phi  = 

normalize(atan2(q.point.y-p.y,  q.pointx-p.x)  -  (qO.theta-PI/2.0)); 

else 

phi  =  normalize(-atan2(q.point.y-p.y,  q.poiuLx-p.x)  + 
(qO.theta+PI^.O)); 

kappaPara  =  cos(phi/2.0)*cos(phi/2.0)*cos^hi/2.0)/size; 
deltaKappa  =  q.kappa  -  kappaPara; 

/♦  ^sildilsitA  ri'pltJiTTiPtJi 

thetaN=((size  >=  0.0)  ?  (qO.theta  -  PI/2.0):(q0.theta  +  PI/2.0)); 
thetaDesire  = 

normalizel((atan2(p.y-q.point.y,  p.x-q.pointx)  +  thctaN)/2.0  -  qO.theta) 

+  qO.theta; 

deltaTheta  =  normalize(q.theta  -  thetaDesire); 

/♦  Calculate  DeltaDist  ♦/ 

dl  =  sqrt((p.x-q.point.x)*(p.x-q,point.x)  +  (p.y-q.point.y)*(p.y-q.point.y)); 
d2  =  -(q.point.x-q0.point.x)*sin(q0.theta) 

+  (q.point.y-q0.point,y)*cos(q0.theta); 
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if  (size  >=  0.0) 

deltaDist  =  d2  -  dl; 

else 

deltaDist  =  d2  +  dl; 

I*  Calculate  Steering  fucnction  =  u  */ 
retum(-(a*deltaKappa  +  b*deltaTheta  +  c*deltaDist)); 

} 

^  <1 9)1  *  *  %  *  iti  4c  ^  *  4t  V  %  4c  *  %  iti  4c  iti  lie  :)( )ti  %  %  itt  :tc  ]tc  *  :tc 4c  1(1  i)c 

FUNCTION:  GetDkappa() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dKappa. 

4c  4c  4c  4c  49  4c  4c  4(  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4t  4c  4c  4c  4<  4c  4c  41  i|c  4c  4c  9f:  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  ^ 

double  GetDkappa(double  u, double  ds) 

1 

retum(u*ds); 

1 

^  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  *  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetKappa() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

Kappa. 

4c  4c  4c  4t  4c  4i  4i  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  ^ 

void  GetKappa(double  dkappa,CONFIGURATION  &q) 

{ 

q.kappa=q.kappa  -f  dkappa; 

} 

^  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c 

FUNCTION:  GetDtheta() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  value  of 

dtheta. 

4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4<  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4(  4c  4c  4c  4c  9):  4c  4c  4c  4c  ^ 

double  GetDtheta(C0NFIGURAT10N  ql, double  ds) 

{ 

retum(ql  .kappa  ♦  ds); 

} 

^4c4c4c4c4c4c4(4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c9^4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c 

FUNCTION:  next() 

Purpose:  This  procedure  is  for  a  function  which  computes  the  next 

configration  of  the  vehicle. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  49  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4‘ 4c  y 

void  next(double  ds,double  dtheta,double  &s, CONFIGURATION  &q) 

{ 

CONHGURATTONql; 
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/*  CONHGURATION  of  ql  */ 
ql.pointx  =  (1.0  -  dtheta*dtheta/6.0)*ds: 
ql.point.y  =  (0.5  -  dtheta*dtheta/24.0)*dtheta*ds; 
ql  .theta  =  dtheta; 


s  =  s  +  ds; 

/*  CONHOURATION  of  q  */ 

q.pointx  =  q.pointx  +  ql.point.x*cos(q.theta)  -  ql .point.y*sin(q.theta); 
q.point.y  =  q.point.y  +  ql.point.x*sin(q.  theta)  +  ql.pomt.y*cos(q.theta): 
q.theta  =  q.theta  +  ql. theta; 

} 


y  3t<  *  :tc  ^  i(c  *  *  Itc  3|c  4t  nc  4c  *  4<  4<  >)<  4c  *  3(1  *  *  *  Ik  #  *  He  ^  *  ill  *  *  4c  :«c  *  «  *  *  *  *  *  He  4c 

FUNCTION:  Openfile() 

Purpose:  This  procedure  opens  the  output  file. 

4c  4c  4c  ck  Ik  He  >!c  Ik  He  He  4c  4c  )k  4c  4c  ck  4c  4c  4c  4c  4c  4c  i(<  He  He  4c  He  He  4c  ck  4>  He  4c  4c  4t  41  Ik  4c  He  4c  He  He  4c  He  He  ik  %  He  4c  He  He  ik  4<  4c  4cik  4c  ik  y 

void  Openfile(CONFIGURATION  q,double  s) 

I 

fpO  =  fopen("path.dat","w"); 
fpl  =  fopen(''path",'’w"); 

fprintf(fpO,"  s  X  y  theta[deg]  kappa  "); 
f]}Rntf(^,"  u  deltaKsqipa  deltaTheta  deltabistNn"); 
printfC  s  X  y  theta[deg]  kappaNn"); 
fprintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4fsn”,s,  q.pointx,  q.pomt.y, 
q.theta*RAD,q  .kappa); 

printf("%4.4f  %4.4f  %4.4f  %4.4f  %4.4f«’’,  s,  q.pointx,  q.pointy, 
q.theta*RAD,q  .kappa); 
fprintf(fpi,"%f  %Ni”,  q.pointx,  q.pointy); 

} 

y4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4'4i4c4>4c4c4c4c4c4c4c4c4i4c4c4c4c4c4c4c4c4i4c4c4c4c4c4c4c4‘4c4c4c4c4c4c4c4c4c4c4c 

FUNCTION:  PrintfileQ 

Purpose:  This  procedure  prints  the  result  to  the  file. 

4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  He  4>  ^ 

void  Printfile(CONFIGURATION  q, double  s) 

{ 

fpiintf(fp0,"%4.4f  %4.4f  %4.4f  %4.4f  %4.4f  ", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q  .kappa); 
printf("%4.4f  %4.4f  %4.4f%4.''f  %4.4fin", 

s,  q.pointx,  q.pointy,  q.theta*RAD,q.kappa); 
fprintf(fpl,"%f  %fin",  q.point.x,  q.point.y);  /*  for  gnuplot  */ 

} 

^4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4i  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4<  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4: 4c  4c  4c  4c  Ik  4c 

FUNCmON:  mainQ 

4c  4c  4c  4c  4c  4c  4c  4c  4: 4c  4c  ik  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4>  4c  4c  %  4c  4c  4c  He  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  4c  y 

void  main(void) 
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CONHGURATION  qO,q; 

POINT  p; 

double  u;  /*  steering  function  */ 
double  DS,s,sO,a,b,c,size; 
double  dkappa.dtheta; 

InputParabola(qO,p); 
size=GetSize(qO,p) ; 

InputInitConfig(q,sO,DS); 

GetConstants(sO,a,b,c); 

s  =  0.0; 

Openfile(q,s); 

do 

{ 

u  =  GetSteering(a,b,c,q,qO,p,size); 
dkappa  =  GetDkappa(u4!)S); 
GetKappa(dkappa,q); 
dtheta=GetDtheta(q,DS); 
next(DS,dtheta,s,q); 

Prin^e(q,s); 

} 

while(s<=2000.0); 

fclose(fpO); 

fclose(fpl); 

} 
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